%***********************************************************
% CS 229 Machine Learning
% Project, Ground truth data generation
%-----------------------------------------------------------
% Date : November 26, 2010
%***********************************************************

clear all;

addpath(genpath('./LIBSVM/'))
addpath(genpath('./KalmanAll/'))

%**************************************************************************
%Input parameters

% Camera dimensions
%outputwidth = 320;
%outputheight = 180;
%Wmax = 640;
%Hmax = 360;

% Scales conversions
scale_conversion_factor_tracking = 1.5;
lecturer_tracking_scale_factor = 640/2560;
saliency_tracking_scale_factor = 640/1920;
board_scale_factor = 640/1920;

num_points = 50000;

% Kalman smoothing parameters
ground_truth_smooth_factor = 150;
final_output_smooth_factor = 250;

% Parameters for panning label prediction
frame = 500; % Sliding window size
numsteps = 5;
step = round(frame/numsteps); % Step size.
derivative_range = 30:30:150;

% Parameters for trajectory regression
frame_offset = 15900;
sample_step = frame_offset/10;

% SVM Parameters
log2c = 0.95;
log2g = 1.80;

% Panning noise removal parameters
pan_label_trough_width = 200;
pan_label_spike_width = 50;

% Rectification look ahead parameter
rectify_future_max = 500;

load groundTruth.dat;
load lecturer.dat;
load saliency.dat;
load boards.dat;
load panning.mat;
%**************************************************************************

dataA = groundTruth;

% The required frame rate is 33 ms per frame. Our data contains observations
% that are taken at time intervals that are multiples of 100 ms. Thus, we
% need to interpolate these observations.

size(dataA)

%--------------------------------------------------------------------------
% Interpolate data
interpolatedTimeStamps = (dataA(1,1):100/3:dataA(size(dataA,1),1));
newDataA = zeros(size(interpolatedTimeStamps',1),3);
newDataA(:,1) = interpolatedTimeStamps';
newDataA(:,2) = (interp1(dataA(:,1)', dataA(:,2)', interpolatedTimeStamps))'./scale_conversion_factor_tracking+10;
newDataA(:,3) = (interp1(dataA(:,1)', dataA(:,3)', interpolatedTimeStamps))'./scale_conversion_factor_tracking+10;

%--------------------------------------------------------------------------
%Adjustment for camera border case
%newDataA(:,2) = min(max(floor(newDataA(:,2) - outputwidth/2),1),Wmax-outputwidth) + outputwidth/2;
%newDataA(:,3) = min(max(floor(newDataA(:,3) - outputheight/2),1),Hmax-outputheight) +outputheight/2 ;
%--------------------------------------------------------------------------

%==========================================================================================
%Prepare features


y = newDataA(:,2);
y = smooth(y,ground_truth_smooth_factor);
l = panning_indicator;
% y,l are the ground truth

%calculate board center in x-direction
board_center_x = (boards(:,1) + boards(:,2))/2;
board_center_x = board_center_x*board_scale_factor;

% Prepare features first

% Scale vectors
%lecuturer feature adjustment
lecturer = lecturer * lecturer_tracking_scale_factor;
%lecturer(:,2) = min(max(floor(lecturer(:,2) - outputwidth/2),1),Wmax-outputwidth) + outputwidth/2 ;
%lecturer(:,3) = min(max(floor(lecturer(:,3) - outputheight/2),1),Hmax-outputheight) +outputheight/2  ;

%saliency feature adjustment
saliency = saliency *  saliency_tracking_scale_factor;

% Prepare all features
X_pan = PreparePanFeatures(1, num_points, frame, step, derivative_range, lecturer, saliency, boards, board_scale_factor);
X_pan = X_pan';
X_reg = PrepareFeatures(1, num_points, frame_offset, sample_step, lecturer, saliency, board_center_x);
X_reg = X_reg';

% MSE values:
MSE_before_panning_filter = 0;
MSE_after_panning_filter = 0;
MSE_final = 0;
sal_MSE = 0;
tracking_MSE = 0;

% Cross validtion ranges
train_range = [1:40000; 1:30000 40001:50000; 1:20000 30001:50000; 1:10000 20001:50000; 10001:50000];
test_range = [40001:50000; 30001:40000; 20001:30000; 10001:20000; 1:10000];

% Code testing ranges
%train_range = [10001:50000];
%test_range = [1:10000];

for seq_range=1:size(train_range,1)
    
    training_seq = train_range(seq_range,:);
    testing_seq = test_range(seq_range,:);
    
    % Step 1: Train the SVM to assign a panning label
    
    tic
    cmd = ['-t 2 -c ', num2str(2^log2c), ' -g ', num2str(2^log2g)];
    model =  svmtrain(l(training_seq), X_pan(training_seq,:) ,cmd);
    
    [predicted_panning_label, accuracy, decision_values] = svmpredict(l(testing_seq), X_pan(testing_seq,:), model);
    
    % The predicted panning labels are quite noisy in places where we expect a
    % '1'. Level these oscillations off to '1'. We do this in two steps:
    % 1. Go over the data once and change all groups of -1's which are thinner
    % than pan_label_trough_width frames in width to 1.
    % 2. Now, go over the data again, and remove all groups of 1's that are
    % less than pan_label_spike_width frames in width.
    
    last_one = 0;
    for i = 1:size(predicted_panning_label, 1)
        if (predicted_panning_label(i,1) == 1)
            if (last_one > 0 && (i - last_one) <= pan_label_trough_width)
                predicted_panning_label(last_one:i,1) = 1;
            end
            last_one = i;
        end
    end

    last_minus_one = 0;
    for i = 1:size(predicted_panning_label, 1)
        if (predicted_panning_label(i,1) == -1)
            if (last_minus_one > 0 && (i - last_minus_one) <= pan_label_spike_width)
                predicted_panning_label(last_minus_one:i,1) = -1;
            end
            last_minus_one = i;
        end
    end
    
    [precision, recall, accuracy] = computePrecisionRecall(predicted_panning_label, l(testing_seq))
    
    toc
   
    t=testing_seq;
    
%     figure(10*seq_range + 1)
%     plot(t,50*l(t),t,50*predicted_panning_label, t,y(t)-mean(y(t)), t,saliency(t,2)-mean(saliency(t,2)), t, lecturer(t,2) - mean(lecturer(t,2)));
%     legend('Ground truth','Prediction','Actual Trajectory', 'saliency', 'lecturer')
%     title(['Precision:' num2str(precision) ' recall: ' num2str(recall) ' accuracy: ' num2str(accuracy) ]);
    
    % Step 2: Apply linear regression
    bestb = glmfit(X_reg(training_seq,:), y(training_seq), 'normal');
    
    %predict on test set
    testX = X_reg(testing_seq,:);
    test_output = [ones(size(testX,1),1) testX]*bestb;
    
    % Analyze vanilla output
    errors = y(testing_seq) - test_output;
    MSE_before_panning_filter =  MSE_before_panning_filter + mean(sum(errors.*errors));
    
%     figure(10*seq_range + 2)
%     plot(t,test_output,t,y(t),'LineWidth',1.8,'MarkerSize',8);
%     grid on;
%     legend('Predicted Traj','Ground Truth')
    
    % Find nearest board when panning state is off and move to it. Look
    % into the future by rectify_future_max frames
    
    num_frames = size(test_output,1);
    test_output_original = zeros(size(test_output, 1), 1);
    test_output_original(:,1) = test_output(:,1);
    
    for i=1:num_frames
        if(i > 1 && predicted_panning_label(i-1,1) == -1) %if panning state is negative
            test_output(i,1) = test_output(i-1,1); %if no pan, repeat last trajectory point
        elseif(predicted_panning_label(i,1) == -1) %first non-pan
            mindist = 1e99;
            for j=1:size(board_center_x,1)
                dist = 0;
                for k=1:min(rectify_future_max, num_frames - i)
                    dist = dist + abs(board_center_x(j,1)-test_output_original(i+k,1));
                end
                if(dist<mindist)
                    mindist = dist;
                    minbcx = board_center_x(j,1);
                end
            end
            test_output(i,1) = minbcx;
        end
    end
    
    errors = y(testing_seq) - test_output;
    MSE_after_panning_filter = MSE_after_panning_filter + mean(sum(errors.*errors));
    
%     figure(10*seq_range + 3)
%     plot(t,test_output,t,y(t),'LineWidth',1.8,'MarkerSize',8);
%     grid on;
    
    % Smoothen output
    test_output = smooth(test_output, final_output_smooth_factor);
    errors = y(testing_seq) - test_output;
    tempMSE = mean(sum(errors.*errors));
    MSE_final = MSE_final + mean(sum(errors.*errors));

%     figure(10*seq_range + 4)
%     plot(t,test_output,t,y(t),'LineWidth',1.8,'MarkerSize',8);
%     grid on;
    
    % Final plot
    figure(10*seq_range + 5)
    plot(t, predicted_panning_label*50, t, y(t), t, test_output);
    title(['MSE:' num2str(tempMSE)]);
    grid on;
    legend('Predicted panning label', 'Ground Truth', 'Predicted Traj')
    
    tracking_errors = y(testing_seq) - lecturer(testing_seq,2);
    sal_errors = y(testing_seq) - saliency(testing_seq,2);
    sal_MSE = sal_MSE + mean(sum(sal_errors.*sal_errors));
    tracking_MSE = tracking_MSE + mean(sum(tracking_errors.*tracking_errors));
    
    % Other plots
    t = testing_seq;
    figure(10*seq_range + 6)
    plot(t,y(t),t,lecturer(t,2),t,saliency(t,2),t,test_output_original);
    grid on;
    legend('y', 'lect', 'sali', 'orig pred')
end

%calculate all MSEs
sal_MSE = sal_MSE./size(train_range,1)
tracking_MSE = tracking_MSE./size(train_range,1)
MSE_before_panning_filter = MSE_before_panning_filter./size(train_range,1)
MSE_after_panning_filter = MSE_after_panning_filter./size(train_range,1)
MSE_final = MSE_final./size(train_range,1)